

#include "glodef.h"
#include "errlog.h"
#include "grid3d.h"
#include "f3io.h"

#include "glovar.h"

#include "soah_common.h"

#ifdef USE_QGRID
#include "g3d_mesq.h"
#endif

#include <stdio.h>
#include <string.h>

static const double MOVE_GRID_LAMBDA=0.000;

node_t *reference_nodes=NULL;
static int reference_nodes_status=0;
int *can_move_nodes=NULL;
fe_matr_p move_grid_matr=NULL;
static int move_grid_matr_status=0;

#define INC_STATUS(name)            ((name ## _status)++)
#define SET_STATUS(name1, name2)    ((name1 ## _status) = (name2 ## _status))
#define CMP_STATUS(name1, name2)    ((name1 ## _status) == (name2 ## _status))

void set_reference_nodes(const node_t *new_nodes)
{
    memcpy(reference_nodes, new_nodes, grid->NN*sizeof(node_t));
    INC_STATUS(reference_nodes);
}

int soah_generate_meshes(int error)
{

    if(!flags.moving_grid)
        return TRUE;

    error_msg("moving_grid is disabled.\n");
    /*
        double min_minq=1.0;
        create_dir(nodes_dir());
        for(curr_ti=0; curr_ti<=total_ti; curr_ti++)
        {
            PrintLogLine(78, '=');
            PrintLog("Time %-10f\n", (double) TIME);

            move_alegrid_laplace(alegrid, TIME);

            grid->MAXH=GridSize(grid, grid->nodes);
            grid->MINQ=GridQuality(grid, grid->nodes);
    #if defined(QGRID_MEMBRANE)
            if((grid->MINQ>=1e-6)&&(grid->MINQ<QGRID_SMOOTH_THRESHOLD))
            {
                fork_QGrid(grid, grid->nodes, alegrid->can_move);
                grid->MAXH=GridSize(grid, grid->nodes);
                grid->MINQ=GridQuality(grid, grid->nodes);
            }
    #endif
            if(grid->MINQ<1e-6)
            {
                if(error)
                    error_msg("Grid quality %g is too low.\n", grid->MINQ);
                else
                    return FALSE;
            }
            PrintLog("t = %-10f: grid: size %g     quality %g\n", (double) TIME,
                     (double) grid->MAXH, (double) grid->MINQ);

            min_minq=fmin(min_minq, grid->MINQ);
            // save_nodes_time(nodes_dir); */ /* the nodes_dir is already open
        save_nodes_time(NULL);
        PRINTF("\rgenerated grid at time %6.3f ", (double) (TIME));
        FFLUSH(stdout);
    }
    PrintLogLine(78, '=');
    PrintLog("minimum grid quality %g\n", min_minq);
    PRINTF("\r%-70s", "generated all grids.");
    return TRUE;
*/
}

#if 0

static int __soah_move_solid_wall__old(void)
{
    double *sg_zX=get_X_vec(sg_z),
        *sg_zY=get_Y_vec(sg_z),
        *sg_zZ=get_Z_vec(sg_z);
    double *sg_xiF=get_F_scal(sg_xi);
    PrintLog("Moving solid walls by lambda * xi * Z\n");
    if(sg_norm<1e-10)
    {
        PrintLog("norm(xi) is too small; nothing to do here.\n");
        return TRUE;
    }

    /* Put reference nodes back into grid */
    if(flags.moving_grid)
        memcpy(grid->nodes, alegrid->ref_nodes, grid->NN*sizeof(node_t));

    /* move solid walls */
    for(int i=0; i<grid->NN; i++)
    {
        node_t *N=grid->nodes+i;
        unsigned on_dont_move=on_ival|on_oval|on_memb|on_pipe;
        if(N->on_bdry&(~on_dont_move))
        {
            N->coords[0]-=sg_lambda*sg_xiF[i]*sg_zX[i];
            N->coords[1]-=sg_lambda*sg_xiF[i]*sg_zY[i];
            N->coords[2]-=sg_lambda*sg_xiF[i]*sg_zZ[i];
        }
    }

    /* for debugging purposes only */
    save_ref_grid();

    /* (maybe) fix quality */
    grid->MAXH=GridSize(grid, grid->nodes);
    grid->MINQ=GridQuality(grid, grid->nodes);
#if defined(QGRID_SOLID)
    if((grid->MINQ>=1e-6)&&(grid->MINQ<QGRID_SMOOTH_THRESHOLD))
    {
        fork_QGrid(grid, grid->nodes, alegrid->can_move);
        grid->MAXH=GridSize(grid, grid->nodes);
        grid->MINQ=GridQuality(grid, grid->nodes);
    }
#endif
    PrintLog("(INFO) Reference grid: size %g     quality %g\n",
        (double)grid->MAXH, (double)grid->MINQ);

    if(grid->MINQ<1e-6)
        return FALSE;

    /* Set new nodes positions as reference */
    if(flags.moving_grid)
        memcpy(alegrid->ref_nodes, grid->nodes, grid->NN*sizeof(node_t));

    save_ref_grid();

    return TRUE;
}

#endif

int soah_move_solid_wall(void)
{
    int ret;
    PrintLog("Moving solid walls by lambda * xi * Z\n");
    if(sg_norm_inf<1e-10)
    {
        PrintLog("norm(xi) is too small; nothing to do here.\n");
        return TRUE;
    }

    /* reassemble matrix if reference nodes have changed */
    if(!CMP_STATUS(reference_nodes, move_grid_matr))
    {
        node_t *save_nodes=grid->nodes;
        grid->nodes=reference_nodes;
        fe_matr_assemble(grid, 1, &move_grid_matr);
        grid->nodes=save_nodes;
        SET_STATUS(move_grid_matr, reference_nodes);
    }

    /* vector of displacement of nodes from reference positions */
    vector disp=alloc_vec(grid->NN);
    /* the right-hand-sice of the system */
    vector disp_rhs=alloc_vec(grid->NN);

    /* assign displacement of boundary nodes according to formula */
    use_vector(disp);
    use_scalar(sg_xi);
    use_vector(sg_z);
    for(int i=0; i<grid->NN; i++)
    {
        if(_is_node_on_sigma(grid, i, sigma_open))
        {
            dispX[i]= -sg_lambda*sg_xiF[i]*sg_zX[i];
            dispY[i]= -sg_lambda*sg_xiF[i]*sg_zY[i];
            dispZ[i]= -sg_lambda*sg_xiF[i]*sg_zZ[i];
        }
    }
    done_vector(sg_z);
    done_scalar(sg_xi);
    done_vector(disp);

    /* the right-hand-side is 0 except at boundary nodes where is it the
     * displacements.  Luckily this is exactly what we have in disp at this time */
    copy_vec(disp, disp_rhs);

    /* solve the linear system.  
     *  TODO: make this cleaner and less ugly */
    {
        scalar U=vec2scal(disp),
            R=vec2scal(disp_rhs),
            T=alloc_scal(3*grid->NN);
        ret=fe_matr_solve_scalar(move_grid_matr, U, R, T, 0.0);
        free_scal(U);
        free_scal(R);
        free_scal(T);
    }

    use_vector(disp);
    for(int i=0; i<grid->NN; i++)
    {
        node_t *gn=grid->nodes+i;
        node_t *rn=reference_nodes+i;
        gn->coords[0]=rn->coords[0]+dispX[i];
        gn->coords[1]=rn->coords[1]+dispY[i];
        gn->coords[2]=rn->coords[2]+dispZ[i];
    }
    done_vector(disp);

    free_vec(disp);
    free_vec(disp_rhs);

    /* for debugging purposes only */
    // save_ref_grid();

    if(!ret)
        return FALSE;
    /* (maybe) fix quality */
    grid->MAXH=GridSize(grid, grid->nodes);
    grid->MINQ=GridQuality(grid, grid->nodes);
    PrintLog("(INFO) Current shape grid: size %g     quality %g\n",
        (double)grid->MAXH, (double)grid->MINQ);
#if defined(QGRID_SOLID)
    //    if((grid->MINQ >= 1e-6) && (grid->MINQ < QGRID_SMOOTH_THRESHOLD))
    if((grid->MINQ<QGRID_SMOOTH_THRESHOLD))
    {
        // fork_QGrid(grid, grid->nodes, can_move_nodes);
        /* QGrid(grid, grid->nodes, alegrid->can_move); */
        mesq_run(mesquite);
        grid->MAXH=GridSize(grid, grid->nodes);
        grid->MINQ=GridQuality(grid, grid->nodes);
        PrintLog("(INFO) Current shape grid: size %g     quality %g\n",
            (double)grid->MAXH, (double)grid->MINQ);
        // save_ref_grid();
    }
#endif

    if(grid->MINQ<1e-6)
        return FALSE;

    /* Set new nodes positions as reference */
    set_reference_nodes(grid->nodes);
    return TRUE;
}

void local_move_grid_matrix(const grid3d_t *grid, int el, unsigned type,
    double ***S)
{
    assert(type==fe_p1_vec);
    if(S==NULL)
        return;

    int i, j;
    double **lS, **lGD;
    static double A[12*12],
        *AA[12]={
                 A, A+12, A+24, A+36,
                 A+48, A+60, A+72, A+84,
                 A+96, A+108, A+120, A+132
    };
    memset(A, 0, sizeof(A));
    *S=AA;
    local_stif(grid, el, type, &lS);
    local_grad_div(grid, el, type, &lGD);
    for(i=0; i<12; i++)
        for(j=0; j<12; j++)
            (*S)[i][j]=lS[i][j]+MOVE_GRID_LAMBDA*lGD[i][j];
}


/****************************************************************************/
/*   move solid walls by just mesquite (without solving PDE)            */
/****************************************************************************/


int soah_move_solid_wall_mesq(void)
{
    PrintLog("Moving solid walls by -lambda*xi*Z using Mesquite\n");
    if(sg_norm_inf<1e-10)
    {
        PrintLog("norm(xi) is too small; nothing to do here.\n");
        return TRUE;
    }

    /* put reference nodes in grid */
    memcpy(grid->nodes, reference_nodes, sizeof(node_t)*grid->NN);

    /* move sigma nodes by -lambda*xi*Z */
    use_scalar(sg_xi);
    use_vector(sg_z);
    for(int i=0;i<grid->NN;i++)
    {
        if(_is_node_on_sigma(grid, i, sigma_open))
        {
            node_t *nd = grid->nodes + i;
            nd->coords[0] -= sg_lambda*sg_xiF[i]*sg_zX[i];
            nd->coords[1] -= sg_lambda*sg_xiF[i]*sg_zY[i];
            nd->coords[2] -= sg_lambda*sg_xiF[i]*sg_zZ[i];
        }
    }
    done_vector(sg_z);
    done_scalar(sg_xi);

    /* for debugging purposes only */
    save_ref_grid();

    /* (maybe) fix quality */
    grid->MAXH=GridSize(grid, grid->nodes);
    grid->MINQ=GridQuality(grid, grid->nodes);
    PrintLog("(INFO) Current shape grid: size %g     quality %g\n",
        (double)grid->MAXH, (double)grid->MINQ);
#if defined(QGRID_SOLID)
    if((grid->MINQ<QGRID_SMOOTH_THRESHOLD))
    {
        mesq_run_no_wrapper(mesquite);
        grid->MAXH=GridSize(grid, grid->nodes);
        grid->MINQ=GridQuality(grid, grid->nodes);
        PrintLog("(INFO) Current shape grid: size %g     quality %g\n",
            (double)grid->MAXH, (double)grid->MINQ);
        // save_ref_grid();
    }
#endif

    if(grid->MINQ<1e-6)
        return FALSE;

    /* Set new nodes positions as reference */
    set_reference_nodes(grid->nodes);
    return TRUE;
}

